#!/usr/bin/env python3
# coding:utf-8
import time
import math

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from turtlesim.srv import Spawn


class FrameListener(Node):
    def __init__(self):
        super().__init__("turtle_tf2_frame_listener")
        self.declare_parameter("target_frame", "turtle1")
        self.target_frame = (
            self.get_parameter("target_frame").get_parameter_value().string_value
        )
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        self.spawn_turtle()
        self.timer = self.create_timer(1, self.on_timer)

    def on_timer(self):
        from_frame_name = self.target_frame
        to_frame_name = "turtle2"
        try:
            now = rclpy.time.Time()
            trans = self.tf_buffer.lookup_transform(to_frame_name, from_frame_name, now)
        except TransformException as ex:
            self.get_logger().info(f"Could not transform from {from_frame_name} to {to_frame_name}: {ex}")
            return
        
        print("Transform 相对位置距离为:x=%f, y=%f "%(trans.transform.translation.x, trans.transform.translation.y))
        msg = Twist()
        scale_rotation_rate = 1
        msg.angular.z = scale_rotation_rate * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
        scale_linear_speed = 0.5
        msg.linear.x = scale_linear_speed * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2)
        self.turtle2_cmdvel_pub.publish(msg)

    """ 新建小乌龟"""
    def spawn_turtle(self):
        # spawn产卵: 新建小乌龟2
        self.spawn_client = self.create_client(Spawn, "spawn")
        # 小乌龟2 控制话题
        self.turtle2_cmdvel_pub = self.create_publisher(Twist, "turtle2/cmd_vel", 1)

        req = Spawn.Request()
        req.name = "turtle2"
        req.x = 2.0
        req.y = 2.0
        req.theta = 0.0
        self.ret = self.spawn_client.call_async(req)

        while rclpy.ok():
            rclpy.spin_once(self)
            if self.ret.done():
                print("新建小乌龟2 成功")
                return
            else:
                print("新建小乌龟2 失败")
            print("请先启动小乌龟")
            time.sleep(1)


def main():
    rclpy.init()
    node = FrameListener()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()


if __name__ == "__main__":
    main()
